/*
 * Yogurt.cpp
 *
 *  Created on: Jun 8, 2011
 *      Author: wilsok17
 */

#include "Yogurt.h"
#include "Search.h"
#include "../Schwartz/Schwartz.h"
#include <iostream>

Yogurt::Yogurt(Grid grid) {
	currentGrid = grid;
	seekerOnePos = pair<string, DIRECTION> ("A", START);
	seekerTwoPos = pair<string, DIRECTION> ("A", START);
}

void Yogurt::run() {
	list<pair<string, DIRECTION> > safePath;
	cout << "Initializing seeker tracking module" << endl;
	Search search;
	SeekerReport seekerRpt;
	list<pair<string, DIRECTION> >::iterator i;
	list<pair<string, DIRECTION> > shortestPath;
	cout << "Initiating wireless communications.  The Schwartz is strong." << endl;
	Schwartz schwartz;

	cout << "Entering execution loop.  Seekers will now be sent instructions" << endl;
	shortestPath = search.findPath(currentGrid, "A", "AK");
	schwartz.sendPath(shortestPath, 1);
	shortestPath = search.findPath(currentGrid, "A", "AK");
	schwartz.sendPath(shortestPath, 2);

	while(safePath.empty()) {
		Message msg = schwartz.retrieveIncomming();
		if(msg.type_ == 1) {
			SeekerReport* rpt = static_cast<SeekerReport*>(&msg);
			receiveSeekerReport(*rpt);
		}

		safePath = findSafePath();
	}

	cout << "Safe path found." << endl;
			for (i = safePath.begin(); i != safePath.end(); i++) {
				cout << "Node:" << i->first << "  Direction:" << currentGrid.directionToString(
						i->second) << endl;
			}


}

void Yogurt::receiveSeekerReport(SeekerReport report) {
	int seekerID = report.seekerID_;
	list<pair<string, DIRECTION> >::iterator it;
	int idx;
	pair<string, DIRECTION> oldPos;
	switch (seekerID) {
	case 1:
		oldPos = seekerOnePos;
		idx = report.lastRouteLegComplete_;
		it = seekerOneRoute.begin();
		for (int k = 0; k <= idx; ++k) {
			++it;
		}
		seekerOnePos = *it;
		//if this path is safe, add it to the safe list and back to the current grid
		if (report.pathStatus_ == 0) {
			if (oldPos.first != seekerOnePos.first) {
//			cout <<oldPos.first << " " << seekerOnePos.first << endl;
			safePaths.addTwoWayEdge(oldPos.first, seekerOnePos);
			currentGrid.addTwoWayEdge(oldPos.first, seekerOnePos);
			}
		} else { //if not add all the remaining seeker paths back to the current grid.
			int count = 0;
			list<pair<string, DIRECTION> >::iterator i;
			string node = seekerOneRoute.begin()->first;
			for (i = seekerOneRoute.begin(); i != seekerOneRoute.end(); ++i) {
				if (count > idx) {
					pair<string, DIRECTION> node2 = pair<string, DIRECTION> (
							i->first, i->second);
					currentGrid.addTwoWayEdge(node, node2);
				}
				++count;
				node = i->first;
			}

		}
		break;
	case 2:
		oldPos = seekerTwoPos;
		idx = report.lastRouteLegComplete_;
		it = seekerTwoRoute.begin();
		for (int k = 0; k <= idx; ++k) {
			++it;
		}
		seekerTwoPos = *it;

		//if this path is safe, add it to the safe list and back to the current grid
		if (report.pathStatus_ == 0) {
			if(oldPos.first != seekerTwoPos.first) {
//			cout <<oldPos.first << " " << seekerTwoPos.first << endl;
			safePaths.addTwoWayEdge(oldPos.first, seekerTwoPos);
			currentGrid.addTwoWayEdge(oldPos.first, seekerTwoPos);
			}
		} else { //if not add all the remaining seeker paths back to the current grid
			int count = 0;
			list<pair<string, DIRECTION> >::iterator i;
			string node = seekerTwoRoute.begin()->first;
			for (i = seekerTwoRoute.begin(); i != seekerTwoRoute.end(); ++i) {
				if (count > idx) {
					pair<string, DIRECTION> node2 = pair<string, DIRECTION> (
							i->first, i->second);
					currentGrid.addTwoWayEdge(node, node2);
				}
				++count;
				node = i->first;
			}

			break;
		}

	}
}

list<pair<string, DIRECTION> > Yogurt::requestInstructions(int seekerID) {
	list<pair<string, DIRECTION> > path;
	switch (seekerID) {
	case 1:
		path = search.findPath(currentGrid, seekerOnePos.first, "AK");
		seekerOneRoute = path;
		break;
	case 2:
		path = search.findPath(currentGrid, seekerTwoPos.first, "AK");
		seekerTwoRoute = path;
		break;
	}

	list<pair<string, DIRECTION> >::iterator j = path.begin();
	list<pair<string, DIRECTION> >::iterator i;
	for (i = path.begin(); i != path.end(); ++i) {
		currentGrid.removeTwoWayEdge(i->first,
				pair<string, DIRECTION> (j->first, i->second));
		if(j == path.end()) {
			break;
		}
		j = i;
	}

	return path;

}

list<pair<string, DIRECTION> > Yogurt::findSafePath() {
	//safePaths.print();
	return search.findPath(safePaths, "A", "AK");
}

Yogurt::~Yogurt() {

}
